﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ArduinoSharp.Lib
{
    public class ArduinoCommand
    {
        const int MaxPower = 255;
        public bool MoveFoward { get; set; }
        public bool MoveBackwar { get; set; }
        public bool RotateRight { get; set; }
        public bool RotateLeft { get; set; }

        public const char FieldDelimiter = ',';
        public const char CommandDelimiter = ';';

        private void Validate()
        {
            if (MoveBackwar && MoveFoward)
            {
                MoveBackwar = false;
                MoveFoward = false;
            }
            if (RotateRight && RotateLeft)
            {
                RotateRight = false;
                RotateLeft = false;
            }
        }
        public void ClearArray(int [] a) 
        {
            for (int i = 0; i <  a.Length; i++)
            {
                a[i] = 0;
            }
        }
        public string GetComandStr()
        {
            Validate();
            var PWM_Array = new int[4]; //rightFront , rightBack , LeftFront, LeftBack

            if (MoveFoward) 
            {
                PWM_Array[0] = MaxPower;
                PWM_Array[2] = MaxPower;
            }
            if (MoveBackwar)
            {
                PWM_Array[1] = MaxPower;
                PWM_Array[3] = MaxPower;
            }
            if (RotateRight)
            {
                PWM_Array[1] = MaxPower;
                PWM_Array[2] = MaxPower;
            }
            if (RotateLeft)
            {
                PWM_Array[0] = MaxPower;
                PWM_Array[3] = MaxPower;
            }

            if (MoveFoward && RotateRight) 
            {
                ClearArray(PWM_Array);
                PWM_Array[0] = MaxPower/2;
                PWM_Array[2] = MaxPower;
            }
            if (MoveFoward && RotateLeft)
            {
                ClearArray(PWM_Array);
                PWM_Array[0] = MaxPower;
                PWM_Array[2] = MaxPower/2;
            }
            if (MoveBackwar && RotateRight)
            {
                ClearArray(PWM_Array);
                PWM_Array[1] = MaxPower/2;
                PWM_Array[3] = MaxPower;
            }
            if (MoveBackwar && RotateLeft)
            {
                ClearArray(PWM_Array);
                PWM_Array[1] = MaxPower;
                PWM_Array[3] = MaxPower/2;
            }

            var command = CommandsPrefixes.MoveComand+FieldDelimiter.ToString();

            for (int i = 0; i < PWM_Array.Length; i++)
            {
                command += PWM_Array[i].ToString();
                command += i == PWM_Array.Length - 1 ? CommandDelimiter.ToString() : FieldDelimiter.ToString();
            }

            //command += GetComandValue(MoveFoward) + FieldDelimiter.ToString();
            //command += GetComandValue(MoveBackwar) + FieldDelimiter.ToString();
            //command += GetComandValue(RotateRight) + FieldDelimiter.ToString();
            //command += GetComandValue(RotateLeft) + CommandDelimiter.ToString();
            return command;
        }


        public static class CommandsPrefixes
        {
            public static int ADC_DATA = 4;

            public static int MoveComand = 5;
            public static int SendAdc = 6;

        }
    }
}
